//*************************************************************************************************
//**************************************CONTROL DE MOTORES*****************************************
//*************************************************************************************************
//Proyecto: AMARANTA
//Autor: Ing. Carlos Felipe Santacruz Rosero         carlos.santacruz@javeriana.edu.co
//Fecha de Modificacin: Julio 2008
//Descripcin: Funciones de implementacin del controlador PID. La estructura del PID se tom de la
//             referencia:           . Se escogio esta por su implementacin sencilla y su claridad
//			   conceptual. Es un PID digital clsico pero la derivada no se calcula del error sin de 
//             la salida de la planta. 



#include <p30f6010.h>    
#include <dsp.h>
#include <ports.h>
//************************************DEFINICIN DE CONSTANTES************************************
#define TOL_ERROR 0.05                    //  Tolerancia del error para el control de posicin

//**********************************VARIABLES GLOBALES******************************************
double debug_error_pos;               // Variable para ver en el debug el error del control de posicion
signed int debug_out_vel;             // Variable para ver en el debug la salida del control de velocidad
double debug_error_vel;               // Variable para ver en el debug el error del control de velocidad 


//*************************************RUTINAS**************************************************
//Rutina para inicializar un sensor 
void vPidSensorCreate(s_sensor * s, unsigned int * sPtr, double factorDet, double factorNum, double delta){

	s -> factorDen = factorDet;
	s -> factorNum = factorNum;
	s -> delta = delta;
	s -> sensorData = sPtr;

}
double dPidSensorNormalize (s_sensor * s){
	
	unsigned int sdata = *(s->sensorData);
	return ( s->factorNum * (  (double) sdata + s->delta ) ) / s->factorDen;
	
}


// Rutina para inicializar una estructura PID:
void vPidInit(s_pid *pid, double kpp, double kii, double kdd, double int_maxx, double int_minn, s_sensor * s, s_sensor * r, double * tolerance)
{
	// Cargar las constantes del controlador en la estructura:
	pid->kp = kpp; 
	pid->ki = kii;
	pid->kd = kdd;
	// Cargar los valores mximos y mnimos de integracin:
	pid->int_max = int_maxx;
	pid->int_min = int_minn;
	// Valores Iniciales:
	pid->out_pas = 0;
	pid->error_acum = 0;
	//Sensor 
	pid->s=s;
	pid->r=r;
	
	pid->tolerance=tolerance;
	
}                        

// Rutina para actualizar el valor del controlador PID:
double dPidSet(s_pid *pid, double referencia, double out)         
{
	
	double error, p_term, i_term, d_term, sum_term;

	error = referencia - out ;                     // Calcular error
	p_term = (pid->kp)*error;                      // Calcula trmino proporcional
	pid->error_acum += error; 	                   // Acumular el error

	if ((pid->error_acum)>(pid->int_max))          // Verificar saturacin por arriba en el integrador
		pid->error_acum = pid->int_max;

	else if((pid->error_acum)<(pid->int_min))     // Verificar saturacin por abajo en el integrador
		pid->error_acum = pid->int_min;

	i_term = (pid->ki)*(pid->error_acum);         // Calcular trmino integral
	d_term = (pid->kd)*(out - (pid->out_pas));    // Calcular trmino diferencial
	pid->out_pas = out;                           // Actualizar valor pasado     
	sum_term = p_term + i_term + d_term;          // Calcular suma de trminos.  

	return sum_term;
	
}

// Rutina que ejecuta los dos controladores:



double dPidControlActions(s_pid *p){
	
	double error,tolerance;
	
	out = vPidSensorNormalize(p->s);
	ref = vPidSensorNormalize(p->r);
	
	error=ref-out;
	
	/* Revisar la tolerancia */
	if (p->tolerance){
		tolerance = *(p->tolerance);
		if (-tolerance<=error&&error<=tolerance)
			out=ref;
	}
	
	return dSetPid(ref,out);
		
}	
	
void vPidControlActions(s_pid * p, int pwmId)
{
	vAmarantaPwmSet(pwmId, (int)dPidControlActions(p));
}




